#include "Shape.h"



Triangle::Triangle(Point3 _p1, Vector3 _v1, Point3 _p2, Vector3 _v2, Point3 _p3, Vector3 _v3) 
		: p1(_p1), v1(_v1), p2(_p2), v2(_v2), p3(_p3), v3(_v3) {
	v1.normalize();
	v2.normalize();
	v3.normalize();
	normal = (p1 - p2)^(p2-p3);
	normal.normalize();
}

void Triangle::draw(){
	glNormal3d(v1[0], v1[1], v1[2]);
	glVertex3d(p1[0], p1[1], p1[2]);
	glNormal3d(v2[0], v2[1], v2[2]);
	glVertex3d(p2[0], p2[1], p2[2]);
	glNormal3d(v3[0], v3[1], v3[2]);
	glVertex3d(p3[0], p3[1], p3[2]);
}

Triangle& Triangle::operator=(const Triangle& a){
	p1 = a.p1;
	p2 = a.p2;
	p3 = a.p3;
	v1 = a.v1;
	v2 = a.v2;
	v3 = a.v3;
	normal = a.normal;
	return *this;
}

HitRecord Triangle::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();
	double den = normal[0]*ray[0] + normal[1]*ray[1] + normal[2]*ray[2];
	if(den != 0){
		double t = -(normal[0]*origin[0] 
					+ normal[1]*origin[1] 
					+ normal[2]*origin[2]
					- (normal[0]*p1[0] + normal[1]*p1[1] + normal[2]*p1[2])) 
					/ den;
		if(t > 0){
            Point3 hit = origin + t * ray;
			Vector3 h1 = hit - p1;
			Vector3 h2 = hit - p2;
			Vector3 h3 = hit - p3;
			Vector3 p12 = p2 - p1;
			Vector3 p23 = p3 - p2;
			Vector3 p31 = p1 - p3;
			double p12l = p12.length();
			double p23l = p23.length();
			double p31l = p31.length();
			p12.normalize();
			p23.normalize();
			p31.normalize();
			double h1d = p12*h1;
			double h2d = p23*h2;
			double h3d = p31*h3;
			if(h1d > 0 && h2d > 0 && h3d > 0
				&& h1d < p12l && h2d < p23l && h3d < p31l)
				result.addHit(t, 0., 0., hit, normal);
		}
	}
	return result;
}


Cube::Cube(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;
	

	Vector3 normalpx = Vector3(1,0,0);
	Vector3 normalnx = Vector3(-1,0,0);
	Vector3 normalpy = Vector3(0,1,0);
	Vector3 normalny = Vector3(0,-1,0);
	Vector3 normalpz = Vector3(0,0,1);
	Vector3 normalnz = Vector3(0,0,-1);

	for(int i=0; i<tessel1; i++){
		double u = static_cast<double>(i);
		for(int j=0; j<tessel1; j++){
			double v = static_cast<double>(j);
			//positive z
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1,0.5), normalpz,
						 Point3(-0.5+u/tessel1,-0.5+v/tessel1,0.5), normalpz,
						 Point3(-0.5+(u+1)/tessel1,-0.5+v/tessel1,0.5), normalpz));
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1,0.5), normalpz,
						 Point3(-0.5+u/tessel1,-0.5+(v+1)/tessel1,0.5), normalpz,
						 Point3(-0.5+u/tessel1,-0.5+v/tessel1,0.5), normalpz));
			//negative z
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1,-0.5+v/tessel1,-0.5), normalnz,
						 Point3(-0.5+u/tessel1,-0.5+v/tessel1,-0.5), normalnz,
						 Point3(-0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1,-0.5), normalnz));
			triangles.push_back(
				Triangle(Point3(-0.5+u/tessel1,-0.5+v/tessel1,-0.5), normalnz,
						 Point3(-0.5+u/tessel1,-0.5+(v+1)/tessel1,-0.5), normalnz,
						 Point3(-0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1,-0.5), normalnz));
			//positive y
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1, 0.5, -0.5+v/tessel1), normalpy,
						 Point3(-0.5+u/tessel1, 0.5, -0.5+v/tessel1), normalpy,
						 Point3(-0.5+(u+1)/tessel1, 0.5, -0.5+(v+1)/tessel1), normalpy));
			triangles.push_back(
				Triangle(Point3(-0.5+u/tessel1, 0.5, -0.5+v/tessel1), normalpy,
						 Point3(-0.5+u/tessel1, 0.5, -0.5+(v+1)/tessel1), normalpy,
						 Point3(-0.5+(u+1)/tessel1, 0.5, -0.5+(v+1)/tessel1), normalpy));
			//negative y
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1, -0.5, -0.5+(v+1)/tessel1), normalny,
						 Point3(-0.5+u/tessel1, -0.5, -0.5+v/tessel1), normalny,
						 Point3(-0.5+(u+1)/tessel1, -0.5, -0.5+v/tessel1), normalny));
			triangles.push_back(
				Triangle(Point3(-0.5+(u+1)/tessel1, -0.5, -0.5+(v+1)/tessel1), normalny,
						 Point3(-0.5+u/tessel1, -0.5, -0.5+(v+1)/tessel1), normalny,
						 Point3(-0.5+u/tessel1, -0.5, -0.5+v/tessel1), normalny));
			//positive x
			triangles.push_back(
				Triangle(Point3(0.5, -0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1), normalpx,
						 Point3(0.5, -0.5+u/tessel1,-0.5+v/tessel1), normalpx,
						 Point3(0.5, -0.5+(u+1)/tessel1,-0.5+v/tessel1), normalpx));
			triangles.push_back(
				Triangle(Point3(0.5, -0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1), normalpx,
						 Point3(0.5, -0.5+u/tessel1,-0.5+(v+1)/tessel1), normalpx,
						 Point3(0.5, -0.5+u/tessel1,-0.5+v/tessel1), normalpx));
			//negative x
			triangles.push_back(
				Triangle(Point3(-0.5, -0.5+(u+1)/tessel1,-0.5+v/tessel1), normalnx,
						 Point3(-0.5, -0.5+u/tessel1,-0.5+v/tessel1), normalnx,
						 Point3(-0.5, -0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1), normalnx));
			triangles.push_back(
				Triangle(Point3(-0.5, -0.5+u/tessel1,-0.5+v/tessel1), normalnx,
						 Point3(-0.5, -0.5+u/tessel1,-0.5+(v+1)/tessel1), normalnx,
						 Point3(-0.5, -0.5+(u+1)/tessel1,-0.5+(v+1)/tessel1), normalnx));

		}
	}
}

Cylinder::Cylinder(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;
	
	Vector3 topNormal = Vector3(0,1,0);
	Vector3 bottomNormal = Vector3(0,-1,0);

	for(int i=0; i<tessel1; i++){
		double r = (static_cast<double>(i))/tessel1;
		//top
		triangles.push_back(
				Triangle(Point3(0, 0.5, 0), topNormal, 
						 Point3(cos((r+1./tessel1)*2*M_PI)/2, 0.5, sin((r+1./tessel1)*2*M_PI)/2), topNormal,
						 Point3(cos(r*2*M_PI)/2, 0.5, sin(r*2*M_PI)/2), topNormal));
		//bottom
		triangles.push_back(
				Triangle(Point3(0, -0.5, 0), bottomNormal, 
						 Point3(cos(r*2*M_PI)/2, -0.5, sin(r*2*M_PI)/2), bottomNormal,
						 Point3(cos((r+1./tessel1)*2*M_PI)/2, -0.5, sin((r+1./tessel1)*2*M_PI)/2), bottomNormal));
		//side
		Vector3 sideNormal1 = Vector3(cos(r*2*M_PI), 0, sin(r*2*M_PI));
		sideNormal1.normalize();
		Vector3 sideNormal2 = Vector3(cos((r+1./tessel1)*2*M_PI), 0, sin((r+1./tessel1)*2*M_PI));
		sideNormal2.normalize();
		for(double i=0.; i<tessel2; i++){
		triangles.push_back(
				Triangle(Point3(cos(r*2*M_PI)/2, -0.5+(i+1)/tessel2, sin(r*2*M_PI)/2), sideNormal1,
						 Point3(cos((r+1./tessel1)*2*M_PI)/2, -0.5+i/tessel2, sin((r+1./tessel1)*2*M_PI)/2), sideNormal2, 
						 Point3(cos(r*2*M_PI)/2, -0.5+i/tessel2, sin(r*2*M_PI)/2), sideNormal1));
		triangles.push_back(
				Triangle(Point3(cos((r+1./tessel1)*2*M_PI)/2, -0.5+(i+1)/tessel2, sin((r+1./tessel1)*2*M_PI)/2), sideNormal2,
						 Point3(cos((r+1./tessel1)*2*M_PI)/2, -0.5+i/tessel2, sin((r+1./tessel1)*2*M_PI)/2), sideNormal2,
						 Point3(cos(r*2*M_PI)/2, -0.5+(i+1)/tessel2, sin(r*2*M_PI)/2), sideNormal1));
		}
	}
}

Cone::Cone(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;

	Vector3 bottomNormal = Vector3(0,-1,0);

	for(int i=0; i<tessel1; i++){
		double r = (static_cast<double>(i))/tessel1;
		//bottom
		triangles.push_back(
				Triangle(Point3(0, -0.5, 0), bottomNormal, 
						 Point3(cos(r*2*M_PI)/2, -0.5, sin(r*2*M_PI)/2), bottomNormal,
						 Point3(cos((r+1./tessel1)*2*M_PI)/2, -0.5, sin((r+1./tessel1)*2*M_PI)/2), bottomNormal));
		//side
		Vector3 sideNormal1 = Vector3(cos(r*2*M_PI), tan(0.5), sin(r*2*M_PI));
		Vector3 sideNormal2 = Vector3(cos((r+1./tessel1)*2*M_PI), tan(0.5), sin((r+1./tessel1)*2*M_PI));
		sideNormal1.normalize();
		sideNormal2.normalize();
		for(int j=0; j<tessel2; j++){
			double u = (static_cast<double>(j));
			triangles.push_back(
				Triangle(
					Point3(
						cos(r*2*M_PI)*(tessel2-u-1)/(2*tessel2), 
						-0.5+(u+1)/tessel2, 
						sin(r*2*M_PI)*(tessel2-u-1)/(2*tessel2)),
					sideNormal1,
					Point3(
						cos((r+1./tessel1)*2*M_PI)*(tessel2-u)/(2*tessel2), 
						-0.5+u/tessel2, 
						sin((r+1./tessel1)*2*M_PI)*(tessel2-u)/(2*tessel2)), 
					sideNormal2, 
					Point3(
						cos(r*2*M_PI)*(tessel2-u)/(2*tessel2), 
						-0.5+u/tessel2, 
						sin(r*2*M_PI)*(tessel2-u)/(2*tessel2)), 
					sideNormal1));
			triangles.push_back(
				Triangle(
					Point3(
							cos((r+1./tessel1)*2*M_PI)*(tessel2-u-1)/(2*tessel2), 
							-0.5+(u+1)/tessel2, 
							sin((r+1./tessel1)*2*M_PI)*(tessel2-u-1)/(2*tessel2)), 
					sideNormal2,
					Point3(
							cos((r+1./tessel1)*2*M_PI)*(tessel2-u)/(2*tessel2), 
							-0.5+u/tessel2, 
							sin((r+1./tessel1)*2*M_PI)*(tessel2-u)/(2*tessel2)), 
					sideNormal2,
					Point3(
							cos(r*2*M_PI)*(tessel2-u-1)/(2*tessel2), 
							-0.5+(u+1)/tessel2, 
							sin(r*2*M_PI)*(tessel2-u-1)/(2*tessel2)), 
					sideNormal1));
		}
	}
}

Sphere::Sphere(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;
	
	double s = 0.5773502692;
	Vector3 v1 = Vector3(s,s,s);
	Vector3 v2 = Vector3(-s,-s,s);
	Vector3 v3 = Vector3(-s,s,-s);
	Vector3 v4 = Vector3(s,-s,-s);

	Point3 origin = Point3(0.,0.,0.);
	Point3 p1 = origin+(v1/2.);
	Point3 p2 = origin+(v2/2.);
	Point3 p3 = origin+(v3/2.);
	Point3 p4 = origin+(v4/2.);

	splitTriangle(Triangle(p3,v3,p2,v2,p1,v1), tessel1);
	splitTriangle(Triangle(p1,v1,p2,v2,p4,v4), tessel1);
	splitTriangle(Triangle(p4,v4,p3,v3,p1,v1), tessel1);
	splitTriangle(Triangle(p2,v2,p3,v3,p4,v4), tessel1);
}



void Sphere::splitTriangle(Triangle t, int times){
	if(times > 8)
		times = 8;
	if(times <= 1)
		triangles.push_back(t);
	else{
		//Vector vc = t
		Vector3 v12 = t.v1+t.v2;
		Vector3 v23 = t.v2+t.v3;
		Vector3 v13 = t.v1+t.v3;

		v12.normalize();
		v23.normalize();
		v13.normalize();

		Point3 origin = Point3(0.,0.,0.);
		Point3 p12 = origin+(v12/2.);
		Point3 p23 = origin+(v23/2.);
		Point3 p13 = origin+(v13/2.);

		splitTriangle(Triangle(t.p1, t.v1, p12, v12, p13, v13), times-1);
		splitTriangle(Triangle(t.p2, t.v2, p23, v23, p12, v12), times-1);
		splitTriangle(Triangle(t.p3, t.v3, p13, v13, p23, v23), times-1);
		splitTriangle(Triangle(p12, v12, p23, v23, p13, v13), times-1);
	}
}

Torus::Torus(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;

	double const R = 0.5;
	double const r = 0.25;

	for(int i = 0; i < tessel1; i++){
		for(double j = 0; j < tessel2; j++){
			double u = (static_cast<double>(i))/tessel1;
			double v = (static_cast<double>(j))/tessel2;

			Point3 p1 = Point3((R+r*cos(v*2*M_PI))*cos(u*2*M_PI), (R+r*cos(v*2*M_PI))*sin(u*2*M_PI), r*sin(v*2*M_PI));
			Point3 p2 = Point3((R+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), r*sin((v+1./tessel2)*2*M_PI));
			Point3 p3 = Point3((R+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), r*sin(v*2*M_PI));
			Point3 p4 = Point3((R+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), r*sin((v+1./tessel2)*2*M_PI));

			Vector3 v1 = Vector3((R+r*cos(v*2*M_PI))*cos(u*2*M_PI), (R+r*cos(v*2*M_PI))*sin(u*2*M_PI), r*sin(v*2*M_PI));
			Vector3 v2 = Vector3((R+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), r*sin((v+1./tessel2)*2*M_PI));
			Vector3 v3 = Vector3((R+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), r*sin(v*2*M_PI));
			Vector3 v4 = Vector3((R+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), r*sin((v+1./tessel2)*2*M_PI));

			triangles.push_back(Triangle(p2,v2,p1,v1,p3,v3));
			triangles.push_back(Triangle(p4,v4,p2,v2,p3,v3));
		}
	}
}

Corkscrew::Corkscrew(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;

	double const R = 0.25;
	double const r = 0.1;
	double const spacing = 0.25;

	for(int i = 0; i < 5*tessel1; i++){
		for(double j = 0; j < tessel2; j++){
			double u = (static_cast<double>(i))/tessel1;
			double v = (static_cast<double>(j))/tessel2;

			Point3 p1 = Point3((R+r*cos(v*2*M_PI))*cos(u*2*M_PI), (R+r*cos(v*2*M_PI))*sin(u*2*M_PI), spacing*u+r*sin(v*2*M_PI));
			Point3 p2 = Point3((R+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), spacing*u+r*sin((v+1./tessel2)*2*M_PI));
			Point3 p3 = Point3((R+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), spacing*(u+1./tessel1)+r*sin(v*2*M_PI));
			Point3 p4 = Point3((R+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), spacing*(u+1./tessel1)+r*sin((v+1./tessel2)*2*M_PI));

			Vector3 v1 = Vector3((R+r*cos(v*2*M_PI))*cos(u*2*M_PI), (R+r*cos(v*2*M_PI))*sin(u*2*M_PI), spacing*u+r*sin(v*2*M_PI));
			Vector3 v2 = Vector3((R+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), spacing*u+r*sin((v+1./tessel2)*2*M_PI));
			Vector3 v3 = Vector3((R+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), spacing*(u+1./tessel1)+r*sin(v*2*M_PI));
			Vector3 v4 = Vector3((R+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (R+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), spacing*(u+1./tessel1)+r*sin((v+1./tessel2)*2*M_PI));

			triangles.push_back(Triangle(p2,v2,p1,v1,p3,v3));
			triangles.push_back(Triangle(p4,v4,p2,v2,p3,v3));
		}
	}
}

Toroid::Toroid(int _tessel1, int _tessel2){
	tessel1 = _tessel1;
	tessel2 = _tessel2;

	double const R = 0.5;
	double const Rf = 0.2;
	double const r = 0.15;

	for(int i = 0; i < 2*tessel1; i++){
		for(double j = 0; j < tessel2; j++){
			double u = (static_cast<double>(i))/tessel1;
			double v = (static_cast<double>(j))/tessel2;

			double Rm0 = Rf*(1+sin((u)*M_PI))+R;
			double Rm1 = Rf*(1+sin((u+1./tessel1)*M_PI))+R;
			
			double h0 = cos(u*M_PI)/2;
			double h1 = cos((u+1./tessel1)*M_PI)/2;

			Point3 p1 = Point3((Rm0+r*cos(v*2*M_PI))*cos(u*2*M_PI), (Rm0+r*cos(v*2*M_PI))*sin(u*2*M_PI), h0+r*sin(v*2*M_PI));
			Point3 p2 = Point3((Rm0+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (Rm0+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), h0+r*sin((v+1./tessel2)*2*M_PI));
			Point3 p3 = Point3((Rm1+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (Rm1+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), h1+r*sin(v*2*M_PI));
			Point3 p4 = Point3((Rm1+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (Rm1+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), h1+r*sin((v+1./tessel2)*2*M_PI));

			Vector3 v1 = Vector3((Rm0+r*cos(v*2*M_PI))*cos(u*2*M_PI), (Rm0+r*cos(v*2*M_PI))*sin(u*2*M_PI), h0+r*sin(v*2*M_PI));
			Vector3 v2 = Vector3((Rm0+r*cos((v+1./tessel2)*2*M_PI))*cos(u*2*M_PI), (Rm0+r*cos((v+1./tessel2)*2*M_PI))*sin(u*2*M_PI), h0+r*sin((v+1./tessel2)*2*M_PI));
			Vector3 v3 = Vector3((Rm1+r*cos(v*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (Rm1+r*cos(v*2*M_PI))*sin((u+1./tessel1)*2*M_PI), h1+r*sin(v*2*M_PI));
			Vector3 v4 = Vector3((Rm1+r*cos((v+1./tessel2)*2*M_PI))*cos((u+1./tessel1)*2*M_PI), (Rm1+r*cos((v+1./tessel2)*2*M_PI))*sin((u+1./tessel1)*2*M_PI), h1+r*sin((v+1./tessel2)*2*M_PI));

			triangles.push_back(Triangle(p2,v2,p1,v1,p3,v3));
			triangles.push_back(Triangle(p4,v4,p2,v2,p3,v3));
		}
	}
}

void Shape::buildDrawList(){
	listName = glGenLists(GLsizei(1));
	
#if _DEBUG
	std::cout << "Making GLList for Shape " << listName
		<< std::endl;
	if(glGetError() != GL_NO_ERROR){
		std::cout << "GL Error" << std::endl;
//		__asm int 3;
	}
#endif

	glNewList(listName, GL_COMPILE_AND_EXECUTE);
	glBegin(GL_TRIANGLES);
	for(std::vector<Triangle>::iterator it = triangles.begin(); it != triangles.end(); it++){
		it->draw();
	}
	glEnd();
	glEndList();

#if _DEBUG
	while(glGetError() != GL_NO_ERROR){
		std::cout << "GL Error" << std::endl;
	//	__asm int 3;
	}
#endif

}

void Shape::draw(){
	if(listName==0)
		buildDrawList();
	else
		glCallList(listName);
}

HitRecord Cube::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();

	double t;
	Point3 hitPoint;

	t = (-0.5 + origin[1])/-ray[1];
	hitPoint = origin + ray*t;
	if(hitPoint[0] <= 0.5 && hitPoint[0] >= -0.5 
		&& hitPoint[2] <= 0.5 && hitPoint[2] >= -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(0., 1., 0.));

	t = (0.5 + origin[1])/-ray[1];
	hitPoint = origin + ray*t;
	if(hitPoint[0] <= 0.5 && hitPoint[0] >= -0.5 
		&& hitPoint[2] <= 0.5 && hitPoint[2] >= -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(0., -1., 0.));

	t = (-0.5 + origin[0])/-ray[0];
	hitPoint = origin + ray*t;
	if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5 
		&& hitPoint[2] <= 0.5 && hitPoint[2] >= -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(1., 0., 0.));

	t = (0.5 + origin[0])/-ray[0];
	hitPoint = origin + ray*t;
	if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5 
		&& hitPoint[2] <= 0.5 && hitPoint[2] >= -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(-1., 0., 0.));

	t = (-0.5 + origin[2])/-ray[2];
	hitPoint = origin + ray*t;
	if(hitPoint[0] < 0.5 && hitPoint[0] > -0.5 
		&& hitPoint[1] < 0.5 && hitPoint[1] > -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(0., 0., 1.));

	t = (0.5 + origin[2])/-ray[2];
	hitPoint = origin + ray*t;
	if(hitPoint[0] < 0.5 && hitPoint[0] > -0.5 
		&& hitPoint[1] < 0.5 && hitPoint[1] > -0.5
		&& t > 0)
		result.addHit(t, 0., 0., hitPoint, Vector3(0., 0., -1.));

	return result;
}

HitRecord Sphere::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();

	double a = ray[0]*ray[0] + ray[1]*ray[1] + ray[2]*ray[2];
	double b = 2*(origin[0]*ray[0] + origin[1]*ray[1] + origin[2]*ray[2]);
	double c = origin[0]*origin[0] + origin[1]*origin[1] + origin[2]*origin[2] - 0.25;

	double t1 = (-b + ::sqrt(b*b - 4*a*c))/(2*a);
	double t2 = (-b - ::sqrt(b*b - 4*a*c))/(2*a);

	if(t1 == 0){
		Point3 hitPoint = origin + ray*t1;
		Vector3 hitNormal = Vector3(hitPoint[0], hitPoint[1], hitPoint[2]);
		hitNormal.normalize();
		result.addHit(t1, 0., 0., hitPoint, hitNormal);
	}
	else if(t1 > 0){
		if(t2 > 0){
			Point3 hitPoint = origin + ray*t1;
			Vector3 hitNormal = Vector3(hitPoint[0], hitPoint[1], hitPoint[2]);
			hitNormal.normalize();
			result.addHit(t1, 0., 0., hitPoint, hitNormal);
			hitPoint = origin + ray*t2;
			hitNormal = Vector3(hitPoint[0], hitPoint[1], hitPoint[2]);
			hitNormal.normalize();
			result.addHit(t2, 0., 0., hitPoint, hitNormal);
			result.sortHits();
		}
		else{
			Point3 hitPoint = origin + ray*t1;
			Vector3 hitNormal = Vector3(hitPoint[0], hitPoint[1], hitPoint[2]);
			hitNormal.normalize();
			result.addHit(t1, 0., 0., hitPoint, hitNormal);
		}
	}
	return result;
}

HitRecord Cylinder::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();

	double a = ray[0]*ray[0] + ray[2]*ray[2];
	double b = 2*(origin[0]*ray[0] + origin[2]*ray[2]);
	double c = origin[0]*origin[0] + origin[2]*origin[2] - 0.25;

	double t1 = (-b + ::sqrt(b*b - 4*a*c))/(2*a);
	double t2 = (-b - ::sqrt(b*b - 4*a*c))/(2*a);

	if(t1 == 0){
		Point3 hitPoint = origin + ray*t1;
		if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
			Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
			hitNormal.normalize();
			result.addHit(t1, 0., 0., hitPoint, hitNormal);
		}
	}
	else if(t1 > 0){
		if(t2 > 0){
			Point3 hitPoint = origin + ray*t1;
			Vector3 hitNormal;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				result.addHit(t1, 0., 0., hitPoint, hitNormal);
			}
			hitPoint = origin + ray*t2;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				result.addHit(t2, 0., 0., hitPoint, hitNormal);
				result.sortHits();
			}
		}
		else{
			Point3 hitPoint = origin + ray*t1;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				result.addHit(t1, 0., 0., hitPoint, hitNormal);
			}
		}
	}

	double tTop = (-0.5 + origin[1])/-ray[1];
	Point3 topHitPoint = origin + ray*tTop;
	if(topHitPoint[0]*topHitPoint[0] + topHitPoint[2]*topHitPoint[2] <= 0.25
		&& tTop > 0)
		result.addHit(tTop, 0., 0., topHitPoint, Vector3(0., 1., 0.));
	
	double tBottom = (0.5 + origin[1])/-ray[1];
	Point3 bottomHitPoint = origin + ray*tBottom;
	if(bottomHitPoint[0]*bottomHitPoint[0] + bottomHitPoint[2]*bottomHitPoint[2] <= 0.25
		&& tBottom > 0)
		result.addHit(tBottom, 0., 0., bottomHitPoint, Vector3(0., -1., 0.));

	return result;
}

HitRecord Cone::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();

	double a = ray[0]*ray[0] + ray[2]*ray[2] - 0.25*ray[1]*ray[1];
	double b = 2*(origin[0]*ray[0] + origin[2]*ray[2]) + 0.5*ray[1]*(0.5 - origin[1]);
	double c = origin[0]*origin[0] + origin[2]*origin[2] - 0.25*(0.5 - origin[1])*(0.5 - origin[1]);

	double t1 = (-b + ::sqrt(b*b - 4*a*c))/(2*a);
	double t2 = (-b - ::sqrt(b*b - 4*a*c))/(2*a);

	if(t1 == 0){
		Point3 hitPoint = origin + ray*t1;
		if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
			Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				hitNormal[1] = tan(0.5);
				hitNormal.normalize();
			result.addHit(t1, 0., 0., hitPoint, hitNormal);
		}
	}
	else if(t1 > 0){
		if(t2 > 0){
			Point3 hitPoint = origin + ray*t1;
			Vector3 hitNormal;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				hitNormal[1] = tan(0.5);
				hitNormal.normalize();
				result.addHit(t1, 0., 0., hitPoint, hitNormal);
			}
			hitPoint = origin + ray*t2;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				hitNormal[1] = tan(0.5);
				hitNormal.normalize();
				result.addHit(t2, 0., 0., hitPoint, hitNormal);
				result.sortHits();
			}
		}
		else{
			Point3 hitPoint = origin + ray*t1;
			if(hitPoint[1] < 0.5 && hitPoint[1] > -0.5){
				Vector3 hitNormal = Vector3(hitPoint[0], 0., hitPoint[2]);
				hitNormal.normalize();
				hitNormal[1] = tan(0.5);
				hitNormal.normalize();
				result.addHit(t1, 0., 0., hitPoint, hitNormal);
			}
		}
	}

	double tBottom = (0.5 + origin[1])/-ray[1];
	Point3 bottomHitPoint = origin + ray*tBottom;
	if(bottomHitPoint[0]*bottomHitPoint[0] + bottomHitPoint[2]*bottomHitPoint[2] <= 0.25
		&& tBottom > 0)
		result.addHit(tBottom, 0., 0., bottomHitPoint, Vector3(0., -1., 0.));

	return result;
}


HitRecord Shape::intersect(const Point3& origin, const Vector3& ray){
	HitRecord result = HitRecord();

	for(std::vector<Triangle>::iterator it = triangles.begin(); it != triangles.end(); it++)
		result.addHits((*it).intersect(origin, ray));

	result.mergeHits();

	return result;
}